/**
 * This file is part of DefectSim.

 *  DefectSim is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.

 *  DefectSim is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.

 *  You should have received a copy of the GNU General Public License
 *  along with DefectSim.  If not, see <http://www.gnu.org/licenses/>.

 */
package defectsim.integrator;

import javax.vecmath.Vector2d;

import repast.simphony.parameter.Parameters;
import repast.simphony.space.continuous.NdPoint;
import defectsim.Globals;
import defectsim.SimulationEnvironment;
import defectsim.Util;
import defectsim.agents.Dislocation;
import defectsim.integrator.velocitylaw.VelocityLaw;
import defectsim.integrator.velocitylaw.VelocityLawFactory;
import defectsim.integrator.velocitylaw.VelocityLawFactory.VelocityLawType;
import defectsim.resources.AllForces;
import defectsim.resources.DefectResource.ForceType;

public class TrapezoidalIntegrator implements Integrator {

	private SimulationEnvironment environment = SimulationEnvironment.getInstance();
	
	private VelocityLaw velocityLaw;
	private Vector2d velocity;
	private Vector2d distance;
	private Vector2d acceleration;
	private boolean climbAllowed;
	/*
	 * Virtual dislocation is used only for integration purposes in order to overcome 
	 * the consequences of iterative approach which requires a change in location 
	 * of the dislocation under integration.
	 */
	private Dislocation virtualDislocation;
	
	public TrapezoidalIntegrator(){
		Parameters p = environment.getParameters();
		int mobLaw = (Integer)p.getValue("mobilitylaw");
		this.climbAllowed = (Boolean)p.getValue("climb");
		switch(mobLaw){
		case 0:
			velocityLaw = VelocityLawFactory.createVelocityLaw(VelocityLawType.Damping);
			break;
		case 1:
			velocityLaw = VelocityLawFactory.createVelocityLaw(VelocityLawType.ThermallyActivated);
			break;
		case 2:
			velocityLaw = VelocityLawFactory.createVelocityLaw(VelocityLawType.ThermalDamping);
			break;
		}
		
	}
	
	public void integrate(Dislocation da, SimulationEnvironment env, Vector2d netForce, AllForces allForces){
		virtualDislocation = da;
		Vector2d externalForce = new Vector2d();
		Vector2d frictionForce = new Vector2d();
		Vector2d interactionForce = new Vector2d();
		Vector2d effectiveForce = new Vector2d();
		Vector2d v = new Vector2d();
		Vector2d d = new Vector2d();

		double rtolerance = 0.01;
		NdPoint oldPosition = env.getLocation(virtualDislocation);
		Vector2d v0 = velocityLaw.getVelocity(da, netForce); 
		int maxiter = 3;
		double dt1 = env.getTimeStep();
		boolean convergent = false;
		while(!convergent){
			Vector2d r = new Vector2d(v0.x * dt1, v0.y * dt1);
			Vector2d v1 = new Vector2d();
			for (int i = 0; i < maxiter; i++) {
				if (!climbAllowed)
					this.moveByVector(r.x * Globals.METER_TO_MICROMETER, Math.toRadians(virtualDislocation.getDirection()));
				else {
					double dr = Math.sqrt(r.x * r.x + r.y * r.y);
					double alfa = Util.calculateAngle(r.x, r.y);
					this.moveByVector(dr * Globals.METER_TO_MICROMETER, Math.toRadians(virtualDislocation.getDirection()) + alfa);
				}

				externalForce = allForces.getForce(ForceType.External).compute(virtualDislocation);
				frictionForce = allForces.getForce(ForceType.Friction).compute(virtualDislocation);
				interactionForce = allForces.getForce(ForceType.Interaction).compute(virtualDislocation);
				
				effectiveForce.add(externalForce, interactionForce);
				if (effectiveForce.x > 0.0)
					netForce.sub(effectiveForce, frictionForce);
				else
					netForce.add(effectiveForce, frictionForce);
				
				v1 = velocityLaw.getVelocity(da, netForce); 

				double err = r.x - (v1.x + v0.x) / 2 * dt1;
				double errmag = Math.abs(err);

				if (errmag < rtolerance){
					convergent = true;
					break;
				}
				else {
					this.moveTo(oldPosition);
					r.add(new Vector2d(-err, -err));
				}
			}
			if (convergent){
				v = v1;
				d = r;
				d.scale(Globals.METER_TO_MICROMETER);
				break;
			}
			else
				dt1 = dt1/2;

		}
		velocity = v;
		distance = d;
		acceleration = new Vector2d();
	}
	
	private void moveByVector(double d, double dir){
		double dx = d * Math.cos(dir);
		double dy = d * Math.sin(dir);
		NdPoint currentPoint = environment.getLocation(virtualDislocation);
		virtualDislocation.setX(currentPoint.getX() + dx);
		virtualDislocation.setY(currentPoint.getY() + dy);
	}
	
	private void moveTo(NdPoint oldP){
		virtualDislocation.setX(oldP.getX());
		virtualDislocation.setY(oldP.getY());
	}

	@Override
	public Vector2d getAcceleration() {
		return acceleration;
	}

	@Override
	public Vector2d getDistance() {
		return distance;
	}

	@Override
	public Vector2d getVelocity() {
		return velocity;
	}
}
